# 已知航向，手搓速度，优化投放和起爆时间
import numpy as np, random, math, pandas as pd, time

# ------------------ 常量和几何 ------------------
g = 9.8
missile_speed = 300.0
cloud_sink = 3.0
cloud_r = 10.0
cloud_duration = 20.0

fake_target = np.array([0.0, 0.0, 0.0])
real_target = np.array([0.0, 200.0, 0.0])   # 真实目标圆柱的轴线通过此点（此处按题意质点化主连线判定）
M1 = np.array([20000.0, 0.0, 2000.0])       # 导弹初始
FY = np.array([6000, -3000, 700])      # 无人机起点

# 航向与速度（可被外部脚本覆盖）
alpha_fixed = 72.46982083 / 180 * math.pi
v_fixed = 100

# 导弹飞行时间（按直线匀速）
missile_dir = fake_target - M1
missile_unit = missile_dir / np.linalg.norm(missile_dir)
missile_flight_time = np.linalg.norm(missile_dir) / missile_speed

# ------------------ 基本几何/物理函数 ------------------
def missile_pos(t: float) -> np.ndarray:
    return M1 + missile_unit * missile_speed * t

def explosion_center(alpha: float, v: float, td: float, f: float) -> np.ndarray:
    """投放后 f 秒爆炸中心位置（投放期/引信期均按水平等速，竖直按自由落体）"""
    dir_xy = np.array([math.cos(alpha), math.sin(alpha)])
    drop = FY + np.array([v * dir_xy[0] * td, v * dir_xy[1] * td, 0.0])
    burst = drop + np.array([v * dir_xy[0] * f, v * dir_xy[1] * f, -0.5 * g * f * f])
    return burst

def cloud_center(alpha: float, v: float, td: float, f: float, tau: float) -> np.ndarray:
    """爆炸后 tau 秒的云团中心（仅考虑下沉，触地后高度不为负）"""
    burst = explosion_center(alpha, v, td, f)
    z = max(0.0, burst[2] - cloud_sink * tau)
    return np.array([burst[0], burst[1], z])

def point_segment_dist(C: np.ndarray, M: np.ndarray, P: np.ndarray) -> float:
    """点 C 到线段 M-P 的最短距离"""
    MP = P - M
    seg2 = float(MP.dot(MP))
    if seg2 <= 1e-12:
        return float(np.linalg.norm(C - M))
    t = float(np.dot(C - M, MP) / seg2)
    t = max(0.0, min(1.0, t))
    proj = M + t * MP
    return float(np.linalg.norm(C - proj))

def blocked_at_time(alpha: float, v: float, td: float, f: float, t: float) -> bool:
    """时刻 t 是否被该枚烟幕遮蔽"""
    te = td + f
    tau = t - te
    if tau < 0.0 or tau > cloud_duration:
        return False
    Mpos = missile_pos(t)
    Cpos = cloud_center(alpha, v, td, f, tau)
    # 判定“导弹—真目标”连线被云团半径 r 覆盖
    d = point_segment_dist(Cpos, Mpos, real_target)
    return d <= cloud_r + 1e-9

# ------------------ 单枚弹评估 ------------------
def single_grenade_shield_time(alpha: float, v: float, td: float, f: float, dt: float = 0.02):
    """返回：(并集遮蔽时长, 区间列表) —— 单枚弹的并集就是它自身"""
    T_end = missile_flight_time
    times = np.arange(0.0, T_end + 1e-9, dt)
    flags = np.zeros_like(times, dtype=bool)
    for i, t in enumerate(times):
        flags[i] = blocked_at_time(alpha, v, td, f, float(t))

    total = float(flags.sum() * dt)

    # 提取连续遮蔽区间
    intervals = []
    in_seg = False
    start = None
    for k, val in enumerate(flags):
        tcur = float(times[k])
        if val and not in_seg:
            in_seg = True; start = tcur
        if (not val) and in_seg:
            intervals.append((start, float(times[k-1] + dt)))
            in_seg = False; start = None
    if in_seg:
        intervals.append((start, float(times[-1] + dt)))

    return total, intervals

# ------------------ 单枚弹优化（模拟退火 + 局部网格细化） ------------------
def optimize_one_grenade(td_bounds=(0.0, 60.0), f_bounds=(0.5, 10.0),
                         dt_eval=0.10, sa_steps=1500, T0=1.2, cooling=0.9995):
    """
    搜索 (td, f) 以最大化单枚烟幕的遮蔽时长。
    约束：td >= 0, f >= 0.5, td+f <= missile_flight_time - 0.01
    """
    # 初始猜测
    cur_td = min(td_bounds[1], 1.5)
    cur_f  = min(f_bounds[1], 3.6)

    # 约束裁剪
    def clamp(td, f):
        td = float(max(td_bounds[0], min(td_bounds[1], td)))
        f  = float(max(f_bounds[0],  min(f_bounds[1],  f )))
        if td + f > missile_flight_time - 0.01:
            # 简单投影：尽量保持 f，收缩 td
            td = min(td, missile_flight_time - 0.01 - f)
            td = max(td_bounds[0], td)
        return td, f

    cur_td, cur_f = clamp(cur_td, cur_f)

    best_td, best_f = cur_td, cur_f
    best_score, _ = single_grenade_shield_time(alpha_fixed, v_fixed, best_td, best_f, dt=dt_eval)
    cur_score = best_score
    T = T0

    for _ in range(sa_steps):
        # 高斯邻域 + 偶尔大步跳转
        if random.random() < 0.05:
            cand_td = random.uniform(*td_bounds)
            cand_f  = random.uniform(*f_bounds)
        else:
            cand_td = cur_td + random.gauss(0, 1.0)
            cand_f  = cur_f  + random.gauss(0, 0.6)

        cand_td, cand_f = clamp(cand_td, cand_f)

        # 评估
        score, _ = single_grenade_shield_time(alpha_fixed, v_fixed, cand_td, cand_f, dt=dt_eval)

        # 接受准则
        if score > cur_score or random.random() < math.exp((score - cur_score) / max(1e-9, T)):
            cur_td, cur_f = cand_td, cand_f
            cur_score = score
            if score > best_score:
                best_score = score
                best_td, best_f = cand_td, cand_f
        T *= cooling

    # 局部网格细化（更精）
    local_best_td, local_best_f, local_best = best_td, best_f, best_score
    grid_td = np.arange(max(td_bounds[0], best_td - 2.0), min(td_bounds[1], best_td + 2.0) + 1e-9, 0.2)
    grid_f  = np.arange(max(f_bounds[0],  best_f  - 1.5), min(f_bounds[1],  best_f  + 1.5) + 1e-9, 0.2)
    for tdg in grid_td:
        for fg in grid_f:
            tdg, fg = clamp(tdg, fg)
            score, _ = single_grenade_shield_time(alpha_fixed, v_fixed, tdg, fg, dt=dt_eval)
            if score > local_best:
                local_best, local_best_td, local_best_f = score, tdg, fg

    return float(local_best_td), float(local_best_f), float(local_best)

# ------------------ 主入口：单枚弹优化并输出 ------------------
def sequential_optimize():
    """
    为兼容你现有的扫参脚本，保持返回形式：
    - drops:  [td]   （长度 1）
    - bursts: [f]    （长度 1）
    - total:  单枚弹的总遮蔽时长（与并集一致）
    - indiv_times:      [total]           （长度 1）
    - indiv_intervals:  [[(s,e), ...]]    （长度 1，内层为区间列表）
    """
    td_bounds = (0.0, min(60.0, missile_flight_time - 1.0))
    f_bounds  = (0.5, 10.0)

    print("优化单枚烟幕弹 ...")
    td, f, s_est = optimize_one_grenade(td_bounds=td_bounds, f_bounds=f_bounds,
                                        dt_eval=0.10, sa_steps=2000, T0=1.2, cooling=0.9995)
    print(f"  初步最优: td={td:.2f}s, f={f:.2f}s, 估计遮蔽={s_est:.2f}s")

    # 更精细评估（dt 更小）
    total, intervals = single_grenade_shield_time(alpha_fixed, v_fixed, td, f, dt=0.02)
    print("\n=== 最终精确评估（单枚） ===")
    print(f"飞行方向 (deg): {math.degrees(alpha_fixed):.2f}, 无人机速度 (m/s): {v_fixed:.1f}")
    print(f"投放={td:.2f}s, 引信={f:.2f}s, 总遮蔽={total:.2f}s, 区间={intervals}")

    # 保存结果
    df = pd.DataFrame({
        "飞行方向(度)": [math.degrees(alpha_fixed)],
        "无人机速度(m/s)": [v_fixed],
        "投放时间(s)": [td],
        "引信延时(s)": [f],
        "遮蔽秒数(精确dt=0.02)": [total],
        "区间列表": [intervals]
    })
    df.to_excel("result3_FY3.xlsx", index=False)
    print("结果已保存到 result3_FY3.xlsx")

    drops = [td]
    bursts = [f]
    indiv_times = [total]
    indiv_intervals = [intervals]
    return drops, bursts, total, indiv_times, indiv_intervals

if __name__ == "__main__":
    random.seed(42); np.random.seed(42)
    t0 = time.time()
    drops, bursts, total, indiv_times, indiv_intervals = sequential_optimize()
    print("总耗时: {:.1f}s".format(time.time() - t0))
